% C \in S^{n(p+1)}
% Minimizes E(||w(t)||_2) - estimation error

function B = constrained_rgr(C, n, p, gamma)
	Omega = inv(C(1:n,1:n));
    ntest = n;
    while(ntest(end)>1e-4)
        
    % Estimate A given Omega
        cvx_begin
            variable X(n*(p+1),n*(p+1)) symmetric
        % construct D
            expression D(n, n*(p+1));
            for i=0:p
                D(1:n,1:n) = D(1:n,1:n) + X((1:n)+n*i,(1:n)+n*i);
            end
            for k=1:p
                for i=0:(p-k)
                    D(1:n,(1:n)+n*k) = D(1:n,(1:n)+n*k) + 2*X((1:n)+n*i,(1:n)+n*(i+k));
                end
            end
        
            minimize(trace(C*X));
            subject to 
                X(1:n,1:n) == min(Omega,Omega');
                X == semidefinite(n*(p+1));
                D(support) == 0;
        cvx_end

        B0 = sqrtm(X(1:n,1:n));
        B1p = B0 \ X(1:n,(1+n):(n*(p+1)));
        A = [eye(n) B0\B1p];
    
    % Estimate Omega given A
        Omega_new = (A*C*A')\eye(n);
        ntest = [ntest norm(Omega-Omega_new,'fro')];
        Omega = Omega_new;
    % Here D(support) == 0; cannot be enforced while choosing Omega given A
    
    end
    B = [B0 B1p];
end
